package com.dji.activationDemo;

import android.app.Activity;
import android.content.SyncAdapterType;
import android.graphics.Bitmap;
import android.graphics.BitmapFactory;
import android.graphics.ImageFormat;
import android.graphics.Rect;
import android.graphics.SurfaceTexture;
import android.graphics.YuvImage;
import android.media.AudioAttributes;
import android.media.AudioManager;
import android.media.MediaFormat;
import android.media.Ringtone;
import android.media.RingtoneManager;
import android.media.SoundPool;
import android.net.Uri;
import android.os.Build;
import android.os.Bundle;
import android.os.Environment;
import android.os.Handler;
import android.util.Log;
import android.view.KeyEvent;
import android.view.TextureView;
import android.view.View;
import android.widget.AdapterView;
import android.widget.Button;
import android.widget.Spinner;
import android.widget.TextView;
import android.widget.Toast;

import androidx.annotation.NonNull;

import com.dji.activationDemo.messaging.WaypointMissionRes;
import com.dji.activationDemo.uitls.BitmapOperation;
import com.dji.activationDemo.uitls.ByteUtil;
import com.dji.activationDemo.uitls.ImageDispose;
import com.dji.activationDemo.uitls.ModuleVerificationUtil;
import com.dji.activationDemo.uitls.RandomNum;
import com.dji.activationDemo.uitls.WriteLog;
import com.dji.activationDemo.websocket.WsImageManager;
import com.dji.activationDemo.websocket.WsManager;


import java.io.ByteArrayInputStream;
import java.io.ByteArrayOutputStream;
import java.io.File;
import java.io.FileNotFoundException;
import java.io.FileOutputStream;
import java.io.FileWriter;
import java.io.IOException;
import java.io.InputStream;
import java.io.OutputStream;
import java.nio.ByteBuffer;
import java.text.SimpleDateFormat;

import dji.common.airlink.SignalQualityCallback;
import dji.common.battery.BatteryState;
import dji.common.camera.ResolutionAndFrameRate;
import dji.common.camera.SettingsDefinitions;
import dji.common.error.DJIError;
import dji.common.flightcontroller.ConnectionFailSafeBehavior;
import dji.common.flightcontroller.FlightControllerState;
import dji.common.flightcontroller.FlightWindWarning;
import dji.common.flightcontroller.ObstacleDetectionSector;
import dji.common.flightcontroller.VisionDetectionState;
import dji.common.flightcontroller.VisionSystemWarning;
import dji.common.flightcontroller.virtualstick.FlightCoordinateSystem;
import dji.common.flightcontroller.virtualstick.RollPitchControlMode;
import dji.common.flightcontroller.virtualstick.VerticalControlMode;
import dji.common.flightcontroller.virtualstick.YawControlMode;
import dji.common.gimbal.GimbalState;
import dji.common.product.Model;
import dji.common.util.CommonCallbacks;
import dji.internal.camera.T;
import dji.internal.util.Util;
import dji.sdk.airlink.AirLink;
import dji.sdk.airlink.LightbridgeLink;
import dji.sdk.base.BaseComponent;
import dji.sdk.base.BaseProduct;
import dji.sdk.battery.Battery;
import dji.sdk.camera.Capabilities;
import dji.sdk.camera.VideoFeeder;
import dji.sdk.codec.DJICodecManager;
import dji.sdk.flightcontroller.FlightController;
import dji.sdk.mission.MissionControl;
import dji.sdk.products.Aircraft;
import dji.sdk.sdkmanager.DJISDKManager;
import dji.sdk.sdkmanager.LiveStreamManager;

public class MainActivity extends Activity implements View.OnClickListener, BatteryState.Callback, DJICodecManager.YuvDataCallback //,TextureView.SurfaceTextureListener
{
    /**
     * 实时数据监听
     */
    private LiveStreamManager.OnLiveChangeListener listener;

    //电池
    int chargeRemainingInPercent = 0;
    private Battery battery;    //= MainActivity.getBatteryInstance();

    //飞行数据
    Double attitude_p;
    Double attitude_y;
    Double attitude_r;

    float gps_al;
    Double gps_la;
    Double gps_long;

    float speed_x;
    float speed_y;
    float speed_z;

    float gimbal_pitch;
    float gimbal_yaw;
    float gimbal_roll;

    int windWarning = 3;
    int systemWarning = 3;


    int uplink = 0;
    int downlink = 0;

    public static FlightController flightController = null;
    public static AirLink airLink = null;


    //关于线程
    private Thread webSocketThread;
    private Thread toWebSocketThread;
    private Thread timeOutWsThread;
    private Thread fpxRoteThread;
    private boolean webSocketFlag = true;

    int fpx_rate = 0;
    int getFpx_rate = 0;
    boolean fpx_status = true;


    protected TextureView mVideoSurface = null;
    private static final String TAG = MainActivity.class.getName();
    protected VideoFeeder.VideoDataListener mReceivedVideoDataListener = null;

    private DJICodecManager mCodecManager;

    private TextView text_drone_info;

    private enum DemoType {USE_TEXTURE_VIEW, USE_SURFACE_VIEW, USE_SURFACE_VIEW_DEMO_DECODER}

    private static DemoType demoType = DemoType.USE_TEXTURE_VIEW;
    RandomNum randomNum = new RandomNum();

    private Button btn_stop_task;
    private Button btn_go_home;

    private String project_id = Constant.PROJECT_ID;
    private String equipment_id = Constant.EQUIPMENT_ID;

    //设置获取流的分辨率
    private Spinner btn_resolution;

    private int fpx_width = 1920;
    private int fpx_height = 1080;

    private Spinner btn_fpx;
    private long fpx = 1000;

    //语音提示播报
    SoundPool soundPool = null;

    private Thread soundThread;
    private boolean sound_status = false;

    @Override
    protected void onCreate(Bundle savedInstanceState) {
        super.onCreate(savedInstanceState);
        WriteLog.writeLog("app打开一次");

        setContentView(R.layout.activity_main);
        initUI();

        // The callback for receiving the raw H264 video data for camera live view
        mReceivedVideoDataListener = new VideoFeeder.VideoDataListener() {
            @Override
            public void onReceive(byte[] videoBuffer, int size) {

            }
        };

        System.out.println("WsManager start");

        battery = DemoApplication.getBatteryInstance();
        battery.setStateCallback(this);
        WsManager.getInstance().init();

        System.out.println("WsManager complete");
        WsImageManager.getInstance().init();
        System.out.println("WsImageManager complete");


        if (demoType == DemoType.USE_SURFACE_VIEW_DEMO_DECODER && isTranscodedVideoFeedNeeded()) {
            System.out.println("provideTranscodedVideoFeed");
            VideoFeeder.VideoFeed standardVideoFeeder = VideoFeeder.getInstance().provideTranscodedVideoFeed();
            standardVideoFeeder.addVideoDataListener(mReceivedVideoDataListener);
        } else if (VideoFeeder.getInstance().getPrimaryVideoFeed() != null) {
            System.out.println("getPrimaryVideoFeed");
            VideoFeeder.getInstance().getPrimaryVideoFeed().addVideoDataListener(mReceivedVideoDataListener);
        }

        setWebSocketThread();
        toWebSocketThread();
        timeOutWs();
        fpxRoteThread();
        soundThread();
        webSocketThread.start();
        toWebSocketThread.start();
        timeOutWsThread.start();
        fpxRoteThread.start();


        final dji.sdk.camera.Camera camera = DemoApplication.getCameraInstance();
        System.out.println("摄像机支持分辨率：");
        ResolutionAndFrameRate[] n = camera.getCapabilities().videoResolutionAndFrameRateRange();
        for (int i = 0; i < n.length; i++) {
            System.out.println(n[i].getResolution());
        }

        playSound();
    }

    SimpleDateFormat format = new SimpleDateFormat("yyyy-MM-dd HH:mm:ss.sss");
    byte[] yuvBytes = null;
    private Handler mHandler = new Handler();
    private Runnable mRunnable = new Runnable() {
        public void run() {
            if (mCodecManager != null) {
                int width = mCodecManager.getVideoWidth();
                int height = mCodecManager.getVideoHeight();
                byte[] bytess = mCodecManager.getYuvData(width, height);
                yuvBytes = bytess;
            }
            mHandler.postDelayed(mRunnable, fpx);  //给自己发送消息，自运行
        }
    };

    private boolean isTranscodedVideoFeedNeeded() {
        if (VideoFeeder.getInstance() == null) {
            return false;
        }

        return VideoFeeder.getInstance().isFetchKeyFrameNeeded() || VideoFeeder.getInstance()
                .isLensDistortionCalibrationNeeded();
    }


    protected void onProductChange() {
        initPreviewer();
    }

    @Override
    public void onResume() {
        super.onResume();
        initPreviewer();
        onProductChange();

        if (mVideoSurface == null) {
            Log.e(TAG, "mVideoSurface is null");
        }


    }

    private void initPreviewer() {

        BaseProduct product = DemoApplication.getProductInstance();

        if (product == null || !product.isConnected()) {
            showToast(getString(R.string.disconnected));
        } else {
            if (null != mVideoSurface) {
                initPreviewerTextureView();
            }
            if (!product.getModel().equals(Model.UNKNOWN_AIRCRAFT)) {
                VideoFeeder.getInstance().getPrimaryVideoFeed().addVideoDataListener(mReceivedVideoDataListener);
            }
        }


    }


    @Override
    public void onPause() {
        super.onPause();
    }

    @Override
    public void onStop() {
        super.onStop();
    }

    public void onReturn(View view) {
        this.finish();
    }

    @Override
    protected void onDestroy() {
        WsManager.ws.sendText("frontend*" + randomNum.genRandomNum() + "*页面关闭");
        WsManager.getInstance().disconnect();
        WsImageManager.getInstance().disconnect();
        mHandler.removeCallbacks(mRunnable);
        webSocketFlag = false;
        if (soundPool!=null)
            soundPool.release();
        WriteLog.writeLog("页面关闭");
        super.onDestroy();
    }


    private void initUI() {
        text_drone_info = (TextView) findViewById(R.id.text_info);
        mVideoSurface = (TextureView) findViewById(R.id.video_previewer_surface);

        btn_stop_task = findViewById(R.id.btn_stop_task);
        btn_go_home = findViewById(R.id.btn_go_home);
        btn_stop_task.setOnClickListener(this);
        btn_go_home.setOnClickListener(this);

        btn_resolution = (Spinner) findViewById(R.id.btn_resolution);
        btn_resolution.setOnItemSelectedListener(new AdapterView.OnItemSelectedListener() {
            @Override
            public void onItemSelected(AdapterView<?> adapterView, View view, int i, long l) {
                String str = (String) btn_resolution.getSelectedItem();
                String[] fpx = str.split("[*]");
                fpx_width = Integer.parseInt(fpx[0]);
                fpx_height = Integer.parseInt(fpx[1]);
            }

            @Override
            public void onNothingSelected(AdapterView<?> adapterView) {

            }
        });

        btn_fpx = findViewById(R.id.btn_fpx);
        btn_fpx.setOnItemSelectedListener(new AdapterView.OnItemSelectedListener() {
            @Override
            public void onItemSelected(AdapterView<?> adapterView, View view, int i, long l) {
                String str = (String) btn_fpx.getSelectedItem();
                fpxModify(str);
            }

            @Override
            public void onNothingSelected(AdapterView<?> adapterView) {

            }
        });
    }

    public void fpxModify(String str) {
        switch (str) {
            case "1HZ":
                fpx = 1000;
                break;
            case "2HZ":
                fpx = 500;
                break;
            case "3HZ":
                fpx = 333;
                break;
            case "4HZ":
                fpx = 250;
                break;
            case "5HZ":
                fpx = 200;
                break;
            case "6HZ":
                fpx = 167;
                break;
            case "7HZ":
                fpx = 143;
                break;
            case "8HZ":
                fpx = 125;
                break;
            case "9HZ":
                fpx = 111;
                break;
            case "10HZ":
                fpx = 100;
                break;

        }
    }

    private void initPreviewerTextureView() {
        TextureView videostreamPreviewTtView = mVideoSurface;
        videostreamPreviewTtView.setSurfaceTextureListener(new TextureView.SurfaceTextureListener() {
            @Override
            public void onSurfaceTextureAvailable(SurfaceTexture surface, int width, int height) {
                Log.d(TAG, "real onSurfaceTextureAvailable");
//                int videoViewWidth = width;
//                int videoViewHeight = height;
//                Log.d(TAG, "real onSurfaceTextureAvailable: width " + videoViewWidth + " height " + videoViewHeight);
                if (mCodecManager == null) {
                    mCodecManager = new DJICodecManager(getApplicationContext(), surface, width, height);
                    //For M300RTK, you need to actively request an I frame.
                    mCodecManager.resetKeyFrame();
//
//                    mCodecManager.enabledYuvData(true);
//                    mCodecManager.setYuvDataCallback(MainActivity.this);
                    //开启线程
                    mHandler.post(mRunnable);

                }
            }

            @Override
            public void onSurfaceTextureSizeChanged(SurfaceTexture surface, int width, int height) {
                int videoViewWidth = width;
                int videoViewHeight = height;
                Log.d(TAG, "real onSurfaceTextureAvailable2: width " + videoViewWidth + " height " + videoViewHeight);
            }

            @Override
            public boolean onSurfaceTextureDestroyed(SurfaceTexture surface) {
                if (mCodecManager != null) {
                    mCodecManager.cleanSurface();
                }
                return false;
            }

            @Override
            public void onSurfaceTextureUpdated(SurfaceTexture surface) {

            }
        });
    }

    @Override
    public void onClick(View v) {
        switch (v.getId()) {
            case R.id.btn_stop_task:
                Constant.waypointExecution = false;
                MissionControl.getInstance().getWaypointMissionOperator().stopMission(new CommonCallbacks.CompletionCallback() {
                    @Override
                    public void onResult(DJIError djiError) {
                        if (djiError != null) {
                            WriteLog.writeLog("航线任务停止失败:" + djiError.getDescription());
                            WsManager.ws.sendText("frontend*" + randomNum.genRandomNum() + "*航线任务停止失败:" + djiError.getDescription());
                        } else {
                            WriteLog.writeLog("航线任务停止");
                            WsManager.ws.sendText("frontend*" + randomNum.genRandomNum() + "*航线任务停止");
                            Constant.waypointStatus = "无任务";
                        }
                    }
                });
                break;
            case R.id.btn_go_home:

                flightController.startGoHome(new CommonCallbacks.CompletionCallback() {
                    @Override
                    public void onResult(DJIError djiError) {
                        if (djiError != null) {
                            WriteLog.writeLog("返航失败:" + djiError.getDescription());
                            WsManager.ws.sendText("frontend*" + randomNum.genRandomNum() + "*返航失败:" + djiError.getDescription());
                        } else {
                            WriteLog.writeLog("返航成功");
                            WsManager.ws.sendText("frontend*" + randomNum.genRandomNum() + "*返航成功");
                        }
                    }
                });
                break;
            default:
                break;
        }
    }

    private boolean isLiveStreamManagerOn() {
        if (DJISDKManager.getInstance().getLiveStreamManager() == null) {
            Toast.makeText(MainActivity.this, "没有流管理", Toast.LENGTH_SHORT).show();
            return false;
        }
        return true;
    }

    int linkNum = 0;

    @Override
    public void onAttachedToWindow() {
        super.onAttachedToWindow();


        BaseProduct product = DemoApplication.getProductInstance();
        if (product == null || !product.isConnected()) {
            Toast.makeText(MainActivity.this, "断开", Toast.LENGTH_SHORT).show();
            return;
        }
        if (isLiveStreamManagerOn()) {
            DJISDKManager.getInstance().getLiveStreamManager().registerListener(listener);
        }
        if (ModuleVerificationUtil.isGimbalModuleAvailable()) {
            DemoApplication.getProductInstance().getGimbal().setStateCallback(new GimbalState.Callback() {
                @Override
                public void onUpdate(@NonNull GimbalState gimbalState) {
                    gimbal_pitch = gimbalState.getAttitudeInDegrees().getPitch();
                    gimbal_yaw = gimbalState.getAttitudeInDegrees().getYaw();
                    gimbal_roll = gimbalState.getAttitudeInDegrees().getRoll();
                }
            });
        }
        if (ModuleVerificationUtil.isFlightControllerAvailable()) {
            flightController =
                    ((Aircraft) DemoApplication.getProductInstance()).getFlightController();
            flightController.setRollPitchControlMode(RollPitchControlMode.VELOCITY);
            flightController.setYawControlMode(YawControlMode.ANGULAR_VELOCITY);
            flightController.setVerticalControlMode(VerticalControlMode.VELOCITY);
            flightController.setRollPitchCoordinateSystem(FlightCoordinateSystem.BODY);

            flightController.setConnectionFailSafeBehavior(ConnectionFailSafeBehavior.GO_HOME, new CommonCallbacks.CompletionCallback() {
                @Override
                public void onResult(DJIError djiError) {

                }
            });

            if (flightController.getFlightAssistant() != null) {
                flightController.getFlightAssistant().setVisionDetectionStateUpdatedCallback(new VisionDetectionState.Callback() {
                    @Override
                    public void onUpdate(@NonNull VisionDetectionState visionDetectionState) {
                        VisionSystemWarning visionSystemWarning = visionDetectionState.getSystemWarning();
                        if (visionSystemWarning == VisionSystemWarning.INVALID) {
                            systemWarning = 0;
                        } else if (visionSystemWarning == VisionSystemWarning.SAFE) {
                            systemWarning = 1;
                        } else if (visionSystemWarning == VisionSystemWarning.DANGEROUS) {
                            systemWarning = 2;
                        } else {
                            systemWarning = 3;
                        }

                    }
                });
            }


            flightController.setStateCallback(new FlightControllerState.Callback() {
                @Override
                public void onUpdate(@NonNull FlightControllerState djiFlightControllerCurrentState) {


                    attitude_p = djiFlightControllerCurrentState.getAttitude().pitch;
                    attitude_y = djiFlightControllerCurrentState.getAttitude().yaw;
                    attitude_r = djiFlightControllerCurrentState.getAttitude().roll;

                    gps_al = djiFlightControllerCurrentState.getAircraftLocation().getAltitude();
                    gps_long = djiFlightControllerCurrentState.getAircraftLocation().getLongitude();
                    gps_la = djiFlightControllerCurrentState.getAircraftLocation().getLatitude();

                    gps_long = Double.isNaN(gps_long) ? 0.0 : gps_long;
                    gps_la = Double.isNaN(gps_la) ? 0.0 : gps_la;

                    speed_x = djiFlightControllerCurrentState.getVelocityX();
                    speed_y = djiFlightControllerCurrentState.getVelocityY();
                    speed_z = djiFlightControllerCurrentState.getVelocityZ();

                    FlightWindWarning flightWindWarning = djiFlightControllerCurrentState.getFlightWindWarning();

//                    System.out.println("windWarning:" + (flightWindWarning == FlightWindWarning.LEVEL_0));
                    if (flightWindWarning == FlightWindWarning.LEVEL_0) {
                        windWarning = 0;
                    } else if (flightWindWarning == FlightWindWarning.LEVEL_1) {
                        windWarning = 1;
                    } else if (flightWindWarning == FlightWindWarning.LEVEL_2) {
                        windWarning = 2;
                    }


                    String description = "电量剩余: " + chargeRemainingInPercent + "\n"
                            + "机身俯仰角: " + attitude_p + "\n"
                            + "机身偏航角: " + attitude_y + "\n"
                            + "机身翻滚角: " + attitude_r + "\n"
                            + "高度: " + gps_al + "\n"
                            + "纬度: " + getWei(gps_la) + "\n"
                            + "经度: " + getWei(gps_long) + "\n"
                            + "风速警告等级: " + windWarning + "\n"
                            + "防撞警告等级: " + systemWarning + "\n"
                            + "Gimbal pitch: " + gimbal_pitch + "\n"
                            + "上行链路信号: " + uplink + "%\n"
                            + "下行链路信号: " + downlink + "%\n"
//                            + "Gimbal yaw: " + gimbal_yaw + "\n"
//                            + "Gimbal roll: " + gimbal_roll + "\n"
//                            + "Velocity x: " + speed_x + "\n"
//                            + "Velocity y: " + speed_y + "\n"
//                            + "Velocity z: " + speed_z + "\n"
                            + "发送速率: " + getFpx_rate + "张/s\n"
                            + "指令信道: " + Constant.webSocketState + "\n"
                            + "图片信道: " + Constant.imageWebSocketState + "\n"
                            + "航点任务: " + Constant.waypointStatus + "\n";
                    changeDescription(description);
                }
            });
        }

        airLink = ((Aircraft) DemoApplication.getProductInstance()).getAirLink();

        airLink.setUplinkSignalQualityCallback(new SignalQualityCallback() {
            @Override
            public void onUpdate(int i) {
                uplink = i;
                if (i < 30 && !sound_status) {
                    WsManager.ws.sendText("frontend*" + randomNum.genRandomNum() + "*无人机上行信号弱，坐标：" + gps_long + "," + gps_la);
//                    soundPool.play(2, 1, 1, 0, 0, 1);
//                    sound_status = true;
//                    soundThread.start();
                }else if (i >= 30){
//                    sound_status = false;
                }
            }
        });

        airLink.setDownlinkSignalQualityCallback(new SignalQualityCallback() {
            @Override
            public void onUpdate(int i) {
                downlink = i;
                if (i < 30 && !sound_status) {
                    WsManager.ws.sendText("frontend*" + randomNum.genRandomNum() + "*无人机下行信号弱，坐标：" + gps_long + "," + gps_la);
//                    soundPool.play(2, 1, 1, 0, 0, 1);
                    sound_status = true;
                    soundThread.start();
                }else if (i >= 30){
                    sound_status = false;
                }
            }
        });

    }

    protected void changeDescription(final String newDescription) {
        text_drone_info.post(new Runnable() {
            @Override
            public void run() {
                text_drone_info.setText(newDescription);
            }
        });
    }


    //电池
    @Override
    public void onUpdate(BatteryState batteryState) {
        chargeRemainingInPercent = batteryState.getChargeRemainingInPercent();
    }


    private void showToast(final String toastMsg) {
        runOnUiThread(new Runnable() {
            @Override
            public void run() {
                Toast.makeText(getApplicationContext(), toastMsg, Toast.LENGTH_LONG).show();

            }
        });
    }

    @Override
    public void onYuvDataReceived(MediaFormat mformat, final ByteBuffer yuvFrame, int dataSize, final int width, final int height) {
        final byte[] bytes = new byte[dataSize];
        yuvFrame.get(bytes);
//        newSaveYuvDataToJPEG(bytes, width, height);
    }


    public static final String IMAGE_PATH = Environment.getExternalStorageDirectory().getAbsolutePath() + "/DJI_ScreenShot";


    // For android API <= 23
    private byte[] oldSaveYuvDataToJPEG(byte[] yuvFrame, int width, int height) {
        if (yuvFrame.length < width * height) {
            //DJILog.d(TAG, "yuvFrame size is too small " + yuvFrame.length);
            return null;
        }

        byte[] y = new byte[width * height];
        byte[] u = new byte[width * height / 4];
        byte[] v = new byte[width * height / 4];
        byte[] nu = new byte[width * height / 4]; //
        byte[] nv = new byte[width * height / 4];

        System.arraycopy(yuvFrame, 0, y, 0, y.length);
        for (int i = 0; i < u.length; i++) {
            v[i] = yuvFrame[y.length + 2 * i];
            u[i] = yuvFrame[y.length + 2 * i + 1];
        }
        int uvWidth = width / 2;
        int uvHeight = height / 2;
        for (int j = 0; j < uvWidth / 2; j++) {
            for (int i = 0; i < uvHeight / 2; i++) {
                byte uSample1 = u[i * uvWidth + j];
                byte uSample2 = u[i * uvWidth + j + uvWidth / 2];
                byte vSample1 = v[(i + uvHeight / 2) * uvWidth + j];
                byte vSample2 = v[(i + uvHeight / 2) * uvWidth + j + uvWidth / 2];
                nu[2 * (i * uvWidth + j)] = uSample1;
                nu[2 * (i * uvWidth + j) + 1] = uSample1;
                nu[2 * (i * uvWidth + j) + uvWidth] = uSample2;
                nu[2 * (i * uvWidth + j) + 1 + uvWidth] = uSample2;
                nv[2 * (i * uvWidth + j)] = vSample1;
                nv[2 * (i * uvWidth + j) + 1] = vSample1;
                nv[2 * (i * uvWidth + j) + uvWidth] = vSample2;
                nv[2 * (i * uvWidth + j) + 1 + uvWidth] = vSample2;
            }
        }
        //nv21test
        byte[] bytes = new byte[yuvFrame.length];
        System.arraycopy(y, 0, bytes, 0, y.length);
        for (int i = 0; i < u.length; i++) {
            bytes[y.length + (i * 2)] = nv[i];
            bytes[y.length + (i * 2) + 1] = nu[i];
        }
        byte[] out_bytes = screenShot(bytes, Environment.getExternalStorageDirectory().getAbsolutePath() + "/DJI_ScreenShot", width, height);
        return out_bytes;
    }


    private byte[] newSaveYuvDataToJPEG(byte[] yuvFrame, int width, int height) {
        if (yuvFrame.length < width * height) {
            //DJILog.d(TAG, "yuvFrame size is too small " + yuvFrame.length);
            return null;
        }
        int length = width * height;

        byte[] u = new byte[width * height / 4];
        byte[] v = new byte[width * height / 4];
        for (int i = 0; i < u.length; i++) {
            v[i] = yuvFrame[length + 2 * i];
            u[i] = yuvFrame[length + 2 * i + 1];
        }
        for (int i = 0; i < u.length; i++) {
            yuvFrame[length + 2 * i] = u[i];
            yuvFrame[length + 2 * i + 1] = v[i];
        }
        byte[] bytes = screenShot(yuvFrame, IMAGE_PATH, width, height);
        return bytes;
    }

    /**
     * Save the buffered data into a JPG image file
     */
    private byte[] screenShot(byte[] buf, String shotDir, int width, int height) {

        System.out.println("width:" + width + ",height:" + height);
        try {
            File dir = new File(shotDir);
            if (!dir.exists() || !dir.isDirectory()) {
                dir.mkdirs();
            }
            YuvImage yuvImage = new YuvImage(buf,
                    ImageFormat.NV21,
                    width,
                    height,
                    null);

            OutputStream outputFile;

            Bitmap bitmap = YuvItoBitmap(yuvImage, width, height);
            Bitmap newBitmap = ImageDispose.zoomBitmap(bitmap, fpx_width, fpx_height);
            byte[] bitBytes = ImageDispose.Bitmap2Bytes(newBitmap);
//            long name = System.currentTimeMillis();
//            final String path = dir + "/ScreenShot_" + name + ".jpg";
//            ImageDispose.getFileFromBytes(bitBytes,path);


//            try {
//                outputFile = new ByteArrayOutputStream();
//            } catch (Exception e) {
//                Log.e(TAG, "test screenShot: new bitmap output file error: " + e);
//                return null;
//            }
//            if (outputFile != null) {
//                yuvImage.compressToJpeg(new Rect(0,
//                        0,
//                        fpx_width,
//                        fpx_height), 80, outputFile);
//            }
//
//            byte[] bytes = null;
//            try {
//                bytes = ((ByteArrayOutputStream) outputFile).toByteArray();
//                outputFile.close();
//            } catch (IOException e) {
//                Log.e(TAG, "test screenShot: compress yuv image error: " + e);
//                e.printStackTrace();
//            }
//
//            OutputStream outputStream = null;
//            long name = System.currentTimeMillis();
//            final String path = dir + "/ScreenShot_" + name + ".jpg";
//            try {
//                outputStream = new FileOutputStream(new File(path));
//            } catch (FileNotFoundException e) {
//                Log.e(TAG, "test screenShot: new bitmap output file error: " + e);
//            }
//            if (outputFile != null) {
//                yuvImage.compressToJpeg(new Rect(0,
//                        0,
//                        fpx_width,
//                        fpx_height), 80, outputStream);
//            }
//            try {
//                outputStream.close();
//            } catch (IOException e) {
//                Log.e(TAG, "test screenShot: compress yuv image error: " + e);
//                e.printStackTrace();
//            }

            return bitBytes;
        } catch (Exception e) {
            WriteLog.writeLog("图片生成时报错：" + e.getMessage());
            return null;
        }
    }

    private Bitmap YuvItoBitmap(YuvImage yuvImage, int width, int height) {
        try {
            if (yuvImage != null) {
                ByteArrayOutputStream stream = new ByteArrayOutputStream();
                yuvImage.compressToJpeg(new Rect(0, 0, width, height), 80, stream);

                Bitmap bmp = BitmapFactory.decodeByteArray(stream.toByteArray(), 0, stream.size());
                //TODO：此处可以对位图进行处理，如显示，保存等
                stream.close();
                return bmp;
            }
        } catch (IOException e) {
            e.printStackTrace();
            return null;
        }
        return null;
    }


    long imageTime;
    long noImageTime;

    public void setWebSocketThread() {
        webSocketThread = new Thread(new Runnable() {
            @Override
            public void run() {
                synchronized (this) {
                    while (webSocketFlag) {
                        if (Thread.currentThread().isInterrupted()) {
                            break;
                        }
                        if (yuvBytes != null) {

                            try {
                                int width = mCodecManager.getVideoWidth();
                                int height = mCodecManager.getVideoHeight();
                                imageTime = System.currentTimeMillis();

                                double mlon = gps_long * 20037508.34 / 180;    //经度转墨卡托
                                double mlat = (Math.log(Math.tan((90 + gps_la) * Math.PI / 360)) / (Math.PI / 180)) * 20037508.34 / 180;   //纬度转墨卡托

//                                if (yuvBytes.length < width * height) {
//                                    return;
//                                }
                                byte bytes[] = oldSaveYuvDataToJPEG(yuvBytes, width, height);
                                int imagebytes = 0;
                                byte[] linkBytes;
                                if (bytes != null) {
                                    linkBytes = new byte[500 + bytes.length];
                                    imagebytes = bytes.length;
                                } else
                                    linkBytes = new byte[500];

                                byte[] timeByte = ByteUtil.getBytes(imageTime); //date转byte[]            8   0-7     long
                                byte[] typeByte = ByteUtil.getBytes(2); //type:1代表drone           4   8-11    int
                                byte[] imageTypeByte = ByteUtil.getBytes(0); //imageType:0代表jpg,1代表yuv   4   12-15  int
                                byte[] widthByte = ByteUtil.getBytes(width);//width                      4   16-19  int
                                byte[] heightByte = ByteUtil.getBytes(height);//height                   4   20-23  int
                                byte[] latByte = ByteUtil.getBytes(gps_la);//lat                         8   24-31  double
                                byte[] lonByte = ByteUtil.getBytes(gps_long);//lon                       8   32-39  double
                                byte[] mlatByte = ByteUtil.getBytes(mlat);//mlat                         8   40-47  double
                                byte[] mlonByte = ByteUtil.getBytes(mlon);//mlon                         8   48-55  double
                                byte[] camPitchByte = ByteUtil.getBytes(gimbal_pitch);//camPitch         4   56-59  float
                                byte[] camRollByte = ByteUtil.getBytes(gimbal_roll);//camRoll            4   60-63  float
                                byte[] camYawByte = ByteUtil.getBytes(gimbal_yaw);//camYaw               4   64-67  float
                                byte[] isSaveByte = ByteUtil.getBytes(1);//isSave:1代表true，0代表false         4   68-71  int
                                System.arraycopy(timeByte, 0, linkBytes, 0, 8);         //0-7
                                System.arraycopy(typeByte, 0, linkBytes, 8, 4);         //8-11
                                System.arraycopy(imageTypeByte, 0, linkBytes, 12, 4);   //12-15
                                System.arraycopy(widthByte, 0, linkBytes, 16, 4);       //16-19
                                System.arraycopy(heightByte, 0, linkBytes, 20, 4);      //20-23
                                System.arraycopy(latByte, 0, linkBytes, 24, 8);         //24-31
                                System.arraycopy(lonByte, 0, linkBytes, 32, 8);         //32-39
                                System.arraycopy(mlatByte, 0, linkBytes, 40, 8);        //40-47
                                System.arraycopy(mlonByte, 0, linkBytes, 48, 8);        //48-55
                                System.arraycopy(camPitchByte, 0, linkBytes, 56, 4);    //56-59
                                System.arraycopy(camRollByte, 0, linkBytes, 60, 4);     //60-63
                                System.arraycopy(camYawByte, 0, linkBytes, 64, 4);      //64-67
                                System.arraycopy(isSaveByte, 0, linkBytes, 68, 4);      //68-71
                                byte[] imageBytes = ByteUtil.getBytes(bytes.length);
                                System.arraycopy(imageBytes, 0, linkBytes, 72, 4);      //72-75
                                byte[] alByte = ByteUtil.getBytes(gps_al);//高度，float            //76-79
                                System.arraycopy(alByte, 0, linkBytes, 76, 4);
                                byte[] attitude_pByte = ByteUtil.getBytes(attitude_p);//机身俯仰角，double            //80-87
                                byte[] attitude_rByte = ByteUtil.getBytes(attitude_r);//机身翻滚角，double            //88-95
                                byte[] attitude_yByte = ByteUtil.getBytes(attitude_y);//机身偏航角，double            //96-103
                                System.arraycopy(attitude_pByte, 0, linkBytes, 80, 8);
                                System.arraycopy(attitude_rByte, 0, linkBytes, 88, 8);
                                System.arraycopy(attitude_yByte, 0, linkBytes, 96, 8);

                                byte[] windWarningByte = ByteUtil.getBytes(windWarning);//风速警告等级
                                System.arraycopy(windWarningByte, 0, linkBytes, 104, 4);      //104-107
                                byte[] systemWarningBye = ByteUtil.getBytes(systemWarning);//防撞警告等级
                                System.arraycopy(systemWarningBye, 0, linkBytes, 108, 4);      //108-111

                                byte[] equipment_idByte = ByteUtil.getBytes(equipment_id);//设备id，String            //112-152
                                byte[] project_idByte = ByteUtil.getBytes(project_id);//项目id，String            //153-191
                                System.arraycopy(equipment_idByte, 0, linkBytes, 112, 41);
                                System.arraycopy(project_idByte, 0, linkBytes, 153, 39);

                                byte[] percentByte = ByteUtil.getBytes(chargeRemainingInPercent);   //电池电量，int  //192-195
                                byte[] uplinkByte = ByteUtil.getBytes(uplink);   //上行链路信号，int  //196-199
                                byte[] downlinkByte = ByteUtil.getBytes(downlink);   //下行链路信号，int  //200-203
                                System.arraycopy(percentByte, 0, linkBytes, 192, 4);
                                System.arraycopy(uplinkByte, 0, linkBytes, 196, 4);
                                System.arraycopy(downlinkByte, 0, linkBytes, 200, 4);

                                if (bytes != null)
                                    System.arraycopy(bytes, 0, linkBytes, 500, imagebytes);     //500-
//                                WsManager.ws.sendBinary(linkBytes);
                                fpx_rate++;
                                WsImageManager.ws.sendBinary(linkBytes);

                                yuvBytes = null;
                            } catch (Exception e) {
                                e.printStackTrace();
                            }
                        }
                    }
                }

            }
        });
    }

    public void toWebSocketThread() {
        toWebSocketThread = new Thread(new Runnable() {
            @Override
            public void run() {
                synchronized (this) {
                    while (webSocketFlag) {
                        if (Thread.currentThread().isInterrupted()) {
                            break;
                        }
                        try {
//                        int width = mCodecManager.getVideoWidth();
//                        int height = mCodecManager.getVideoHeight();
                            noImageTime = System.currentTimeMillis();

                            double mlon = 0;
                            double mlat = 0;
                            if (gps_long != null && gps_la != null) {
                                mlon = gps_long * 20037508.34 / 180;    //经度转墨卡托
                                mlat = (Math.log(Math.tan((90 + gps_la) * Math.PI / 360)) / (Math.PI / 180)) * 20037508.34 / 180;   //纬度转墨卡托
                            }

                            byte[] linkBytes;
                            linkBytes = new byte[500];

                            byte[] timeByte = ByteUtil.getBytes(noImageTime); //date转byte[]            8   0-7     long
                            byte[] typeByte = ByteUtil.getBytes(2); //type:1代表drone           4   8-11    int
                            byte[] imageTypeByte = ByteUtil.getBytes(0); //imageType:0代表jpg,1代表yuv   4   12-15  int
//                        byte[] widthByte = ByteUtil.getBytes(width);//width                      4   16-19  int
//                        byte[] heightByte = ByteUtil.getBytes(height);//height                   4   20-23  int
                            byte[] latByte = ByteUtil.getBytes(gps_la);//lat                         8   24-31  double
                            byte[] lonByte = ByteUtil.getBytes(gps_long);//lon                       8   32-39  double
                            byte[] mlatByte = ByteUtil.getBytes(mlat);//mlat                         8   40-47  double
                            byte[] mlonByte = ByteUtil.getBytes(mlon);//mlon                         8   48-55  double
                            byte[] camPitchByte = ByteUtil.getBytes(gimbal_pitch);//camPitch         4   56-59  float
                            byte[] camRollByte = ByteUtil.getBytes(gimbal_roll);//camRoll            4   60-63  float
                            byte[] camYawByte = ByteUtil.getBytes(gimbal_yaw);//camYaw               4   64-67  float
                            byte[] isSaveByte = ByteUtil.getBytes(1);//isSave:1代表true，0代表false         4   68-71  int
                            System.arraycopy(timeByte, 0, linkBytes, 0, 8);         //0-7
                            System.arraycopy(typeByte, 0, linkBytes, 8, 4);         //8-11
                            System.arraycopy(imageTypeByte, 0, linkBytes, 12, 4);   //12-15
//                        System.arraycopy(widthByte, 0, linkBytes, 16, 4);       //16-19
//                        System.arraycopy(heightByte, 0, linkBytes, 20, 4);      //20-23
                            System.arraycopy(latByte, 0, linkBytes, 24, 8);         //24-31
                            System.arraycopy(lonByte, 0, linkBytes, 32, 8);         //32-39
                            System.arraycopy(mlatByte, 0, linkBytes, 40, 8);        //40-47
                            System.arraycopy(mlonByte, 0, linkBytes, 48, 8);        //48-55
                            System.arraycopy(camPitchByte, 0, linkBytes, 56, 4);    //56-59
                            System.arraycopy(camRollByte, 0, linkBytes, 60, 4);     //60-63
                            System.arraycopy(camYawByte, 0, linkBytes, 64, 4);      //64-67
                            System.arraycopy(isSaveByte, 0, linkBytes, 68, 4);      //68-71
                            byte[] imageBytes = ByteUtil.getBytes(0);
                            System.arraycopy(imageBytes, 0, linkBytes, 72, 4);      //72-75
                            byte[] alByte = ByteUtil.getBytes(gps_al);//高度，float            //76-79
                            System.arraycopy(alByte, 0, linkBytes, 76, 4);
                            byte[] attitude_pByte = ByteUtil.getBytes(attitude_p);//机身俯仰角，double            //80-87
                            byte[] attitude_rByte = ByteUtil.getBytes(attitude_r);//机身翻滚角，double            //88-95
                            byte[] attitude_yByte = ByteUtil.getBytes(attitude_y);//机身偏航角，double            //96-103
                            System.arraycopy(attitude_pByte, 0, linkBytes, 80, 8);
                            System.arraycopy(attitude_rByte, 0, linkBytes, 88, 8);
                            System.arraycopy(attitude_yByte, 0, linkBytes, 96, 8);

                            byte[] windWarningByte = ByteUtil.getBytes(windWarning);//风速警告等级
                            System.arraycopy(windWarningByte, 0, linkBytes, 104, 4);      //104-107
                            byte[] systemWarningBye = ByteUtil.getBytes(systemWarning);//防撞警告等级
                            System.arraycopy(systemWarningBye, 0, linkBytes, 108, 4);      //108-111

                            byte[] equipment_idByte = ByteUtil.getBytes(equipment_id);//设备id，String            //112-152
                            byte[] project_idByte = ByteUtil.getBytes(project_id);//项目id，String            //153-191
                            System.arraycopy(equipment_idByte, 0, linkBytes, 112, 41);
                            System.arraycopy(project_idByte, 0, linkBytes, 153, 39);

                            byte[] percentByte = ByteUtil.getBytes(chargeRemainingInPercent);   //电池电量，int  //192-195
                            byte[] uplinkByte = ByteUtil.getBytes(uplink);   //上行链路信号，int  //196-199
                            byte[] downlinkByte = ByteUtil.getBytes(downlink);   //下行链路信号，int  //200-203
                            System.arraycopy(percentByte, 0, linkBytes, 192, 4);
                            System.arraycopy(uplinkByte, 0, linkBytes, 196, 4);
                            System.arraycopy(downlinkByte, 0, linkBytes, 200, 4);


                            WsManager.ws.sendBinary(linkBytes);
//                        WriteLog.writeLog("发送不带图消息成功");
                            Thread.sleep(500);
                        } catch (Exception e) {
                            e.printStackTrace();
                        }

                    }
                }

            }
        });
    }

    public void fpxRoteThread() {
        fpxRoteThread = new Thread(new Runnable() {
            @Override
            public void run() {
                while (fpx_status) {
                    try {
                        if (Constant.imageWebSocketState.equals("连接成功")) {
                            getFpx_rate = fpx_rate;
                            fpx_rate = 0;
                        } else {
                            getFpx_rate = 0;
                        }
                        Thread.sleep(1000);
                    } catch (InterruptedException e) {
                        e.printStackTrace();
                    }
                }
            }
        });
    }

    public void timeOutWs() {
        timeOutWsThread = new Thread(new Runnable() {
            @Override
            public void run() {
                try {
                    while (true) {
                        long timeOut = System.currentTimeMillis();
                        if ((timeOut - imageTime) > 30000) {
                            imageTime = timeOut;
//                            webSocketThread.interrupt();
//                            WsManager.getInstance().disconnect();
                            WsImageManager.getInstance().disconnect();
                            Thread.sleep(500);
//                            WsManager.getInstance().init();
                            WsImageManager.getInstance().init();
//                            webSocketThread.start();
                            WriteLog.writeLog("图片消息：因为websocket堵塞，重启服务");
                            System.out.println("图片消息堵塞");
                            continue;

                        } else if ((timeOut - noImageTime) > 30000) {
                            noImageTime = timeOut;
//                            toWebSocketThread.interrupt();
                            WsManager.getInstance().disconnect();
                            Thread.sleep(500);
                            WsManager.getInstance().init();
//                            toWebSocketThread.start();
                            WriteLog.writeLog("无图片消息：因为websocket堵塞，重启服务");
                            System.out.println("无图片消息堵塞");
                            continue;
                        }
                        Thread.sleep(5000);
                    }

                } catch (InterruptedException e) {
                    e.printStackTrace();
                }

            }
        });

    }


    public static String getWei(double d) {
        java.text.DecimalFormat df = new java.text.DecimalFormat("#.#######");
        return df.format(d);
    }


    public void playSound() {
        try {
            if (Build.VERSION.SDK_INT >= 21) {
                SoundPool.Builder builder = new SoundPool.Builder();
                //传入音频的数量
                builder.setMaxStreams(5);
                //AudioAttributes是一个封装音频各种属性的类
                AudioAttributes.Builder attrBuilder = new AudioAttributes.Builder();
                //设置音频流的合适属性
                attrBuilder.setLegacyStreamType(AudioManager.STREAM_MUSIC);
                builder.setAudioAttributes(attrBuilder.build());
                soundPool = builder.build();
            } else {
                //第一个参数是可以支持的声音数量，第二个是声音类型，第三个是声音品质
                soundPool = new SoundPool(1, AudioManager.STREAM_SYSTEM, 5);
            }

            soundPool.load(getAssets().openFd("wrjsqlj.mp3"), 1);
            soundPool.load(getAssets().openFd("slfh.mp3"), 2);
            soundPool.load(getAssets().openFd("slwffh.mp3"), 3);
            soundPool.load(getAssets().openFd("xhr.mp3"), 4);

            soundPool.setOnLoadCompleteListener(new SoundPool.OnLoadCompleteListener() {
                @Override
                public void onLoadComplete(SoundPool soundPool, int sampleId, int status) {
                    Toast.makeText(MainActivity.this, "语音文件加载完毕", Toast.LENGTH_SHORT).show();
                }
            });
        } catch (IOException e) {
            e.printStackTrace();
        }
    }

    public void soundThread() {
        soundThread = new Thread(new Runnable() {
            @Override
            public void run() {
                while (sound_status) {
                    try {
                        soundPool.play(2, 1, 1, 0, 0, 1);
                        Thread.sleep(2000);
                    } catch (InterruptedException e) {
                        e.printStackTrace();
                    }
                }
            }
        });
    }


}